#include <stdio.h>
#include <stdlib.h>

#include "../hle.h"

#include "__SysMemUserForUser.h"
#include "__ThreadManForUser.h"
#include "../blkalloc.h"
#include "../core.h"
#include "../thread.h"

Context curCallbackCtx;
SceUID curCallback = 0;
SceUID cbids[NUM_CBTYPES] = {0};

SceUID currentThread = -1;
SceUID threadqueue[64]; /* TODO: STATIC */
u32 numthreads = 0;

/* Addresses of the fake return syscalls */
u32 cbRetAddr = 0xdeadc0de;
u32 thRetAddr = 0xdeadc0de;

SceUID curModule;

/*************
 * CALLBACKS *
 *************/

void __KernelSetCbRetAddr(u32 addr)
{
    cbRetAddr = addr;
}

void __KernelReturnFromCallback()
{
    Callback *c = ko_get(curCallback);
    if (c)
    {
        __KernelLoadContext(&curCallbackCtx);
#ifndef NO_THREAD
        cs_enter(&intTh_mtx);
        intTh_state = INTTH_READY;
        thread_cond_signal(intTh_ready);
        cs_leave(&intTh_mtx);
#endif
        core_run();
        __KernelReSchedule();
    }
    else
        _log(ERR, HLE, "__KernelReturnFromCallback(%d): wrong callback!", curCallback);

    curCallback = 0;
}

u8 __KernelInCallback()
{
#ifndef NO_THREAD
    return (intTh_state == INTTH_RUNNING);
#else
    return (curCallback != 0);
#endif
}

void __KernelCallCallback(SceUID id)
{
    /* First, make sure we're on the right thread */
    Callback *c = ko_get(id);
    if (c && c->threadId == __KernelGetCurThread())
    {
        _log(INF, HLE, "__KernelCallCallback(%d) => count %08x arg %08x addr %08x", id, c->notifyCount, c->notifyArg, c->argumentAddr);

        __KernelSaveContext(&curCallbackCtx);

        /* Clear the current context and set the good PC */
        cpu.r[A0] = c->notifyCount;
        cpu.r[A1] = c->notifyArg;
        cpu.r[A2] = c->argumentAddr;
        cpu.r[RA] = cbRetAddr;
        c->notifyCount = 0;
        c->notifyArg = 0;
        cpu.pc = c->entrypoint;
        curCallback = id;
    }
    else
        _log(ERR, HLE, "__KernelCallCallback(%d): not on the right thread");
}

/* Used by GE callbacks, since it uses (given argument, start argument) instead of (notify count, notify argument, start argument); in fact, it's an interrupt */
void __KernelCallCallbackWithArg(SceUID id, u32 argument)
{
    Callback *c = ko_get(id);
    if (c)
    {
        _log(INF, HLE, "__KernelCallCallbackWithArg(%d, %08x)", id, argument);

        __KernelSaveContext(&curCallbackCtx);

        cpu.r[A0] = argument;
        cpu.r[A1] = c->argumentAddr;
        cpu.r[RA] = cbRetAddr;
        cpu.pc = c->entrypoint;
        curCallback = id;
    }
    else
        _log(ERR, HLE, "__KernelCallCallbackWithArg(%d): not on the right thread");
}

void __KernelCallCallbackType(CbType type)
{
    if (cbids[type])
        __KernelCallCallback(cbids[type]);
}

void __KernelCallCallbackTypeWithArg(CbType type, u32 arg)
{
    if (cbids[type])
        __KernelCallCallbackWithArg(cbids[type], arg);
}

#ifndef NO_THREAD
void __KernelCallCallbackFromGfxTh(SceUID id)
{
    cs_enter(&intTh_mtx);
    intTh_state = INTTH_MUSTSTART;
    intTh_cbId = id;
    intTh_cbHasArg = 0;
    cs_leave(&intTh_mtx);
}

void __KernelCallCallbackWithArgFromGfxTh(SceUID id, u32 arg)
{
    cs_enter(&intTh_mtx);
    intTh_state = INTTH_MUSTSTART;
    intTh_cbId = id;
    intTh_cbHasArg = 1;
    intTh_cbArg = arg;
    cs_leave(&intTh_mtx);
}

void __KernelCallCallbackTypeFromGfxTh(CbType type)
{
    SceUID id;
    cs_enter(&intTh_mtx);
    id = cbids[type];
    cs_leave(&intTh_mtx);
    if (id)
        __KernelCallCallbackFromGfxTh(id);
}

void __KernelCallCallbackTypeWithArgFromGfxTh(CbType type, u32 arg)
{
    SceUID id;
    cs_enter(&intTh_mtx);
    id = cbids[type];
    cs_leave(&intTh_mtx);
    if (id)
        __KernelCallCallbackWithArgFromGfxTh(id, arg);
}

void __KernelWaitCallbackEndFromGfxTh()
{
    cs_enter(&intTh_mtx);
    if (intTh_state != INTTH_READY)
        thread_cond_wait(intTh_ready, &intTh_mtx);
    cs_leave(&intTh_mtx);
}
#endif

void __KernelNotifyCallbackType(CbType type, u32 arg)
{
    if (cbids[type])
    {
        Callback *c = ko_get(cbids[type]);
        c->notifyCount++;
        c->notifyArg = arg;
        __KernelCallCallback(cbids[type]);
    }
}

void __KernelRegisterCallback(CbType type, SceUID cbid)
{
    cbids[type] = cbid;
}

void __KernelUnregisterCallback(CbType type)
{
    cbids[type] = 0;
}

u8 __KernelCallbackExists(SceUID id)
{
    if (ko_get(id))
        return 1;
    else
        return 0;
}

SceUID __KernelCreateCallback(char *name, u32 entrypoint, u32 argument)
{
    Callback *c = malloc(sizeof(Callback));
    SceUID id = ko_init(c, KO_CALLBACK);
    _log(INF, HLE, "%d = sceKernelCreateCallback(name = \"%s\", entry = %08x)", id, name, entrypoint);

    c->size = sizeof(Callback);
    c->name[0] = '\0';
    strncpy(c->name, name, 32);

    c->entrypoint = entrypoint;
    c->threadId = __KernelGetCurThread();
    c->argumentAddr = argument;
    c->notifyCount = 0;
    c->notifyArg = 0;

    return id;
}

/***************
 * EVENT FLAGS *
 ***************/

u8 __KernelEventFlagMatches(u32 *pattern, u32 bits, u8 wait, u32 outAddr)
{
    if ((wait & PSP_EVENT_WAITOR)
         ? (bits & *pattern) /* one or more bits of the mask */
        : ((bits & *pattern) == bits)) /* all the bits of the mask */
    {
        if (mem_isOk(outAddr))
            mem_write32(outAddr, *pattern);

        if (wait & PSP_EVENT_WAITCLEAR)
            *pattern &= ~bits;
        return 1;
    }
    return 0;
}       

/***********
 * THREADS *
 ***********/

void __KernelThreadingInit()
{
}

void __KernelThreadingShutdown()
{
    s32 i;
    currentThread = -1;
    numthreads = 0;
    for (i = 0; i < 64; i++)
        threadqueue[i] = -1;
}

u32 __KernelGetWaitValue(SceUID threadID)
{
    SceKernelThread *t = ko_get(threadID);
    if (t)
        return t->waitValue;
    else {
        _log(ERR, THREAD, "__KernelGetWaitValue(%d): bad thread", threadID);
        return 0;
    }
}

void __KernelResumeThread(SceUID threadID)
{
    SceKernelThread *t = ko_get(threadID);
    if (t)
    {
        t->nt.status &= ~THREADSTATUS_WAIT;
        if (!(t->nt.status & (THREADSTATUS_SUSPEND | THREADSTATUS_WAIT)))
            t->nt.status |= THREADSTATUS_READY;
        t->delayed = 0;
    }
    else
        _log(ERR, THREAD, "__KernelResumeThread(%d): bad thread", threadID);
}

void __KernelSetThRetAddr(u32 addr)
{
    thRetAddr = addr;
}

/* Saves the current CPU context */
void __KernelSaveContext(Context *c)
{
    s32 i;
    for (i = 0; i < 32; i++) {
        c->r[i] = cpu.r[i];
        c->f[i] = cpu.f[i];
    }

    for (i = 0; i < 128; i++)
        c->v[i] = cpu.v[i];

    for (i = 0; i < 15; i++)
        c->vfpuCtrl[i] = cpu.vfpuCtrl[i];

    c->hi = cpu.hi;
    c->lo = cpu.lo;
    c->fcsr = cpu.fcsr;
    c->pc = cpu.pc;
}

void __KernelSaveThreadContext(SceUID id)
{
    SceKernelThread *t = ko_get(id);
    if (t) {
        __KernelSaveContext(&t->context);
        _log(TRC, THREAD, "Context saved: %d - %s", id, t->nt.name);
    }
    else
        _log(ERR, THREAD, "__KernelSaveThreadContext(%d): bad thread", id);
}

/* Loads a CPU context */
void __KernelLoadContext(Context *c)
{
    s32 i;
    for (i = 0; i < 32; i++) {
        cpu.r[i] = c->r[i];
        cpu.f[i] = c->f[i];
    }

    for (i = 0; i < 128; i++)
        cpu.v[i] = c->v[i];

    for (i = 0; i < 15; i++)
        cpu.vfpuCtrl[i] = c->vfpuCtrl[i];

    cpu.hi = c->hi;
    cpu.lo = c->lo;
    cpu.fcsr = c->fcsr;
    cpu.pc = c->pc;
}

void __KernelLoadThreadContext(SceUID id)
{
    SceKernelThread *t = ko_get(id);
    if (t)
    {
        __KernelLoadContext(&t->context);
        currentThread = id;
        _log(TRC, THREAD, "Context loaded: %d - %s", id, t->nt.name);
    }
    else
        _log(ERR, THREAD, "__KernelLoadThreadContext(%d): bad thread", id);
}

/* Resumes waiting threads
 * type: wait type
 * id: depends on the wait type; thread id for WAITTTHREAD, semaphore id, ..
 * test: type of the test used with id */
u8 __KernelResumeThreads(WaitType type, s32 id, ResumeType test)
{
    u8 doneAnything = 0;
    u32 i;

    for (i = 0; i < numthreads; i++)
    {
        SceKernelThread *t = ko_get(threadqueue[i]);
        if (t && t->nt.status & THREADSTATUS_WAIT && t->nt.waitType == type)
        {
            u8 cond = 1;
            switch (test)
            {
            case RESUMETYPE_NONE:
                cond = 1;
                break;
            case RESUMETYPE_ID:
                cond = (t->nt.waitID == id);
                break;
            case RESUMETYPE_VALUE_OR:
                cond = (t->waitValue & id) == (u32)id;
                break;
            }
            if (cond)
            {
                __KernelResumeThread(threadqueue[i]);
                if (t->nt.status & THREADSTATUS_READY)
                    doneAnything = 1;
            }
        }
    }

    return doneAnything;
}

/* Delays a thread, that will be restarted when the delay time outs */
void __KernelDelayCurThread(u32 usec, u8 processCallbacks)
{
    SceKernelThread *t = ko_get(currentThread);
    t->delayed = 1;
    t->delayTimer = host_initTimer(usec / 1000, 0);
    t->isProcessingCallbacks = processCallbacks;
    t->nt.status = THREADSTATUS_WAIT;
    __KernelReSchedule();
}

/* Resumes delayed thread when delay is finished */
u8 __KernelResumeDelayedThreads()
{
    u8 doneAnything = 0;
    u32 i;

    for (i = 0; i < numthreads; i++)
    {
        SceKernelThread *t = ko_get(threadqueue[i]);
        if (t && t->delayed && host_testTimer(&t->delayTimer))
        {
            __KernelResumeThread(threadqueue[i]);
            if (t->nt.status & THREADSTATUS_READY)
                doneAnything = 1;
        }
    }

    return doneAnything;
}

/* Makes the current thread wait for an event */
void __KernelWaitCurThread(WaitType type, SceUID id, u32 value, u32 timeout, u8 processCallbacks)
{
    SceKernelThread *t = ko_get(currentThread);
    t->nt.status = THREADSTATUS_WAIT;
    t->nt.waitID = id;
    t->nt.waitType = type;
    t->nt.numReleases++;
    t->waitValue = value;
    t->isProcessingCallbacks = processCallbacks;
    if (timeout) {
        t->delayed = 1;
        t->delayTimer = host_initTimer(timeout / 1000, 0);
    }

    RETURN(0);
    __KernelReSchedule();
}

void __KernelReSchedule()
{
    s32 bestThread = -1;

    s32 next = 0;
    u32 i;
    if (__KernelInCallback()) {
        _log(INF, THREAD, "In callback - cancelling reschedule");
        return;
    }

    _log(TRC, THREAD, "--- ReSchedule ---");

    for (i = 0; i < numthreads; i++)
    {
        if (currentThread == threadqueue[i]) {
            next = i;
            break;
        }
    }

    _log(TRC, THREAD, "Actual thread: %d at %08x", next, cpu.pc);
    __KernelResumeDelayedThreads();

    for (i = 0; i < numthreads; i++)
    {
        SceKernelThread *t;

        next = (next + 1) % numthreads;
        t = ko_get(threadqueue[next]);

        if (t)
        {
            if (t->nt.status & THREADSTATUS_READY && bestThread == -1) {
                _log(TRC, THREAD, "- Thread %d [%d] [%-20s] selected with priority %08x", next, threadqueue[next], t->nt.name, t->nt.currentPriority);
                bestThread = next;
            }
            else
                _log(TRC, THREAD, "- Thread %d [%d] [%-20s] not selected [%s; priority 0x%08x]", next, threadqueue[next],
                        t->nt.name, t->nt.status & THREADSTATUS_READY ? "ready" : "not ready", t->nt.currentPriority);
        }
        else
            _log(ERR, THREAD, "- Thread %d invalid?", next);
    }

    if (bestThread != -1)
    {
        __KernelSaveThreadContext(currentThread);
        __KernelLoadThreadContext(threadqueue[bestThread]);

        _log(TRC, THREAD, "=> Starting thread %d at %08x", bestThread, cpu.pc);
        core_run();
    }
    else
    {
        core_wait();
        if (__KernelResumeDelayedThreads())
            __KernelReSchedule();
    }

    _log(TRC, THREAD, "------------------");
}

SceUID __KernelCreateThread(const char *name, u32 entryPoint, u32 priority, u32 stacksize, u32 attr)
{
    u32 i;
    SceUID id;
    SceKernelThread *t = malloc(sizeof(SceKernelThread));
    char blkName[15] = {'\0'};

    id = ko_init((void*)t, KO_THREAD);

    threadqueue[numthreads++] = id;

    for (i = 0; i < 32; i++) {
        t->context.r[i] = 0;
        t->context.f[i] = 0.0f;
    }
    for (i = 0; i < 128; i++)
        t->context.v[i] = 0.0f;
    for (i = 0; i < 15; i++)
        t->context.vfpuCtrl[i] = 0x00000000;

    t->context.vfpuCtrl[VFPU_CTRL_PFXS] = 0xe4; /* passthru */
    t->context.vfpuCtrl[VFPU_CTRL_PFXT] = 0xe4; /* passthru */
    t->context.vfpuCtrl[VFPU_CTRL_CC] = 0x3f; 

    t->context.hi = 0;
    t->context.lo = 0;
    t->context.pc = entryPoint;
    t->context.fcsr = 0;

    memset(&t->nt, 0xcd, sizeof(t->nt));
    t->nt.attr = attr;
    t->nt.initialPriority = t->nt.currentPriority = priority;
    t->nt.stackSize = stacksize;
    t->nt.status = THREADSTATUS_DORMANT;
    t->nt.waitType = WAITTYPE_NONE;
    t->nt.waitID = 0;
    t->waitValue = 0;
    t->nt.exitStatus = 0;
    t->nt.numInterruptPreempts = 0;
    t->nt.numReleases = 0;
    t->nt.numThreadPreempts = 0;
    t->nt.runForClocks.lo = 0;
    t->nt.runForClocks.hi = 0;
    t->nt.wakeupCount = 0;
    t->delayed = 0;
    strncpy(t->nt.name, name, 32);
    t->context.r[GP] = __KernelGetModuleGP(curModule);
    t->context.r[RA] = thRetAddr;
    /* Initialize stack */
    snprintf(blkName, 14, "stack [%d]", id);
    t->stackaddr = blkAlloc_alloc(&userMemory, stacksize, 1, blkName, 1);
    t->nt.initialStack = t->stackaddr + stacksize;
    t->context.r[SP] = t->context.r[K0] = t->nt.initialStack - 512;
    for (i = 0; i < stacksize; i++)
        mem_write8(t->stackaddr + i, 0);
    return id;
}

u32 __KernelDeleteThread(SceUID id)
{
    SceKernelThread *t = ko_get(id);
    if (t)
    {
        u32 i, tid;
        tid = 0;
        _log(INF, THREAD, "Removing thread %d", id);

        for (i = 0; i < numthreads; i++)
            if (threadqueue[i] == id)
                tid = i;

        for (i = tid; i < numthreads; i++)
            threadqueue[i] = threadqueue[i + 1];

        numthreads--;

        blkAlloc_free(&userMemory, t->stackaddr);
        ko_free(id);
        return 0;
    }
    else {
        _log(ERR, THREAD, "__KernelDeleteThread(%d): bad thread", id);
        return SCE_ERROR_NOT_FOUND_THREAD;
    }
}

void __KernelSetupRootThread(SceUID moduleID, s32 args, const char *argp, s32 prio, u32 stacksize, s32 attr) 
{
    u32 location;
    s32 i;
    SceKernelThread *t;
    Module *m = ko_get(moduleID);
    if (m->entry_addr == 0xffffffff)
        return;
    if (m->encrypted)
        return;
    curModule = moduleID;
    /* grab mips regs */
    if (currentThread != -1)
        __KernelSaveThreadContext(currentThread);
    cpu.pc = m->entry_addr;
    currentThread = __KernelCreateThread("rootthread", m->entry_addr, prio, stacksize, attr);
    t = ko_get(currentThread);
    t->nt.status = THREADSTATUS_READY;

    strncpy(t->nt.name, "rootthread", 10);

    __KernelLoadThreadContext(currentThread);
    cpu.r[A0] = args;
    cpu.r[SP] -= 256;
    location = cpu.r[SP];
    cpu.r[A1] = location;

    if (!mem_isOk(location)) {
        _log(WRN, HLE, "Wrong address %08x for the arguments - skipping");
        return;
    }
        
    for (i = 0; i < args; i++)
        mem_write8(location + i, argp[i]);
}

SceUID __KernelGetCurThread()
{
    return currentThread;
}

/* This one is fake but necessary :) */
void __KernelReturnFromThread()
{
    SceKernelThread *t;

    _log(INF, THREAD, "__KernelReturnFromThread");

    t = ko_get(currentThread);
    if (t) {
        t->nt.status = THREADSTATUS_DORMANT;
        t->nt.exitStatus = t->context.r[V0];
    }
    else
        _log(ERR, THREAD, "Invalid current thread!", currentThread);

    /* Find threads that waited for me
     *  Wake them */
    __KernelResumeThreads(WAITTYPE_THREADEND, currentThread, RESUMETYPE_ID);
    __KernelReSchedule();
}

